from visual import *
from visual.graph import *
from visual.controls import *
import serial
import sys
try:
	ser = serial.Serial('/dev/ttyUSB0', 115200)
	#ser=open('imu.collect','r')
except serial.serialutil.SerialException, errorstring:
	 print errorstring
	 sys.exit(0)
	

t=0
cu=[[]]
def angletras(x,in_min,in_max,out_min,out_max):
	if x>=0:
		return ((2*pi)/500)*x
	else:
		return ((2*pi)/250)*x

def change(a): # Called by controls when button clicked
	cu[a][1]= not cu[a][1] 

'''Setto la finestra di controllo'''
c = controls(title='Controlli',width=300, height=300,x=0,y=520)
bottoni = [
button( pos=(-8,-8), width=30, height=30, text='GX', action=lambda:change(0)),
button( pos=(24,-8),width=30, height=30, text='GY', action=lambda:change(1)),
button( pos=(56,-8), width=30, height=30, text='GZ', action=lambda:change(2)),
button( pos=(-8,24), width=30, height=30, text='AX', action=lambda:change(3)),
button( pos=(24,24), width=30, height=30, text='AY', action=lambda:change(4)),
button( pos=(56,24), width=30, height=30, text='AZ', action=lambda:change(5))
]

gyro=gdisplay(title='GYRO',x=0,y=0,width=600, height=500)
cu=[
[gcurve(color=color.red),True],
[gcurve(color=color.blue),True],
[gcurve(color=color.green),True]]
acce=gdisplay(title='ACCE',x=520,y=0,width=600, height=500)
cu.append([gcurve(display=acce,color=color.red),True])
cu.append([gcurve(gdisplay=acce,color=color.blue),True])
cu.append([gcurve(gdisplay=acce,color=color.green),True])

dis=display(title='CUBO',width=300, height=300,x=420,y=520)
b=box(length=5, height=5, width=5)
ser.readline()
ser.readline()
l=ser.readline().split('\t')
primo=map(int,l[:-1])
mmm=min(primo)
media=[0,0,0,0,0,0]
numval=0
angoli=[0,0,0]
while True:
	c.interact() # Check for mouse; drive actions
	l=ser.readline().split('\t')
	values=map(int,l[:-1])
	#print values
	rate(50)

	ind=0
	numval+=1
	for ag in values:
		actualval=ag
		#if ind >=3:		
		media[ind]=actualval+media[ind]
		actualval=actualval-media[ind]/numval
		if ind<3:
				newangle=angletras(actualval,-250,500,-360,360)
				if abs(angoli[ind]-newangle)>=1:
					b.rotate(angle=newangle, axis=(ind==0,ind==1,ind==2), origin=(ind==0,ind==1,ind==2))
				else:
				   newangle=0
				angoli[ind]=newangle
		if (cu[ind])[1]:
			(cu[ind])[0].plot(pos=(t,actualval))
		ind+=1
	t += 0.1		

